%author: wxj233
%time: 2023.11.12 10:00
%function:  特征点

classdef FeaturePoint < handle
    %FEATUREPOINT 特征点
    %   用于记录每一个轨迹的特征点
    
    properties
        id  % 特征点唯一标识
        points  % 特征点轨迹,其中points的结构为[t, x, vx, y, vy, id, cluster],id为真实目标id
        Xk_e  % 特征点当前估计值
        Pk_ee  % 特征点估计误差协方差
        lifeMax  % 特征点最大生命值
        isValidAss  % 是否为有效关联,每当与一个真实量测关联跟新该值赋值true，否则为false

        fplife  % 特征点生命值，从上次有效量测以来，每记忆一次减一，有效量测一次恢复最大生命值
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一
        track  % 该特征点所属轨迹

        assP  % 用于状态跟新的关联点[t, x, y, id, cluster],没有则赋值为[]
    end
    
    methods
        function obj = FeaturePoint(lifeMax, points, Xk_e, Pk_ee, track)
            %FEATUREPOINT 构造此类的实例
            % lifeMax: 特征点最大生命值
            % points: 初始轨迹，长度为2[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye; ...]
            % Xk_e: 初始状态
            % Pk_ee: 初始估计误差协方差矩阵
            % track: 当前特征点所属轨迹
            obj.id = obj.geId();
            obj.lifeMax = lifeMax;
            obj.points = points;
            obj.Xk_e = Xk_e;
            obj.Pk_ee = Pk_ee;
            obj.track = track;
            obj.assP = [];

            obj.isValidAss = true;
            obj.mNum = 2;
            obj.fplife = lifeMax;
        end

        function costs = costs(obj, points, inDoor, dr, dtheta, dvr)
            % 计算某些点属于该特征点轨迹的代价
            % points：待计算的点[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % dr: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 径向速度量测方差
            % inDoor: 落入波门中的概率
            % return: 代价值，（这个地方是每个点的概率密度值，实际上是收益值）
            H = [1, 0, 0, 0;
                 0, 0, 1, 0];
            % 量测误差协方差矩阵
            R = [dr, 0, 0;
                 0, dtheta, 0;
                 0, 0, dvr];
            Xk_p = obj.Xk_e;  % 下一个状态预测值（因为状态已经换成了预测状态）
            Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵

            [Zk_p, Pz, ~] = obj.CuT(@obj.h, Xk_p, Pk_pe, 3);
            Vks = points(:, [2,3,4])' - Zk_p;  % 新息（量测残差）
            Sk = Pz+R;  % 新息协方差

            costs = [];  % 代价矩阵,取距离为代价
            for i = 1:1:size(Vks, 2)
                Vk = Vks(:, i);
                chi = Vk'/Sk*Vk;  %  三个自由度的卡方分布
                p = 0;
                if chi < chi2inv(inDoor, 3)
                    p = 1/((2*pi)^(3/2)*sqrt(det(Sk))) * exp(-0.5*chi);
                end
                costs = [costs, p];
            end
        end

        function [A, Q] = predict(obj, dt, q)
            % 状态预测
            % dt: 下一个时刻与当前时刻的时间差
            % q: 过程噪声数量级
            A = [1, dt, 0, 0;
                 0, 1, 0, 0;
                 0, 0, 1, dt;
                 0, 0, 0, 1];
            % 过程噪声
            Q = q*[1, 0, 0, 0;
                   0, 1, 0, 0;
                   0, 0, 1, 0;
                   0, 0, 0, 1];
            Xk_p = A*obj.Xk_e;  % 下一个状态预测值
            Pk_pe = A*obj.Pk_ee*A' + Q;  % 预测误差协方差矩阵
%             Pk_pe = A*obj.Pk_ee*A';  % 预测误差协方差矩阵

            obj.Xk_e = Xk_p;
            obj.Pk_ee = Pk_pe;
        end


        function [Kk, R, isExist] = update_new(obj, point, dr, dtheta, dvr, nowTime, Zk_p, Pz, Pxz)
            % 状态更新,此方法会完成状态跟新，如果point为空则为记忆，若生命值为0则从track中删除该特征点
            % point: 用于状态更新的量测[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % dr: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 速度量测方差
            % nowTime: 当前时刻
            % Zk_p: 量测预测值
            % Pz: 量测预测方差
            % Pxz: 量测预测协方差

            % 量测误差协方差矩阵
            R = [dr, 0, 0;
                 0, dvr, 0;
                 0, 0, dtheta];

            isExist = true;  % 标记当前特征点是否还存在
            if isempty(point)
                obj.fplife = obj.fplife - 1;
                obj.isValidAss = false;
                if obj.fplife == 0
                    isExist = false;
                    obj.track.deleteFp(obj.id);
                end
                r = norm([obj.Xk_e(1,1), obj.Xk_e(3,1)]);
                theta = atan2(obj.Xk_e(3,1), obj.Xk_e(1,1));
                vr = obj.Xk_e([2,4],1)'*obj.Xk_e([1,3],1)/r;
                x = obj.Xk_e(1,1);
                y = obj.Xk_e(3,1);
                vx = obj.Xk_e(2,1);
                vy = obj.Xk_e(4,1);
                % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye]
                obj.points = [obj.points; [nowTime, r, theta, vr, x, y, vx, vy, 0, x, y, -1, obj.Xk_e']];
                Kk = zeros([4,3]);
            else
                Zk = point(1, 2:4)';
                Xk_p = obj.Xk_e;  % 下一个状态预测值
                Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵
    
                Vk = Zk - Zk_p;  % 新息（量测残差）
%                 Sk = H*Pk_pe*H'+R;  % 新息协方差
                Sk = Pz + R;  % 新息协方差,Pz就是量测预测的协方差，通过不敏变换计算可得
    
%                 Kk = Pk_pe*H'/Sk;  % 卡尔曼增益
                Kk = Pxz/Sk;  % Pxz是Xk_pe和Zk_p之间的协方差矩阵,通过不敏变换计算得到
                obj.Xk_e = Xk_p + Kk*Vk;
%                 obj.Pk_ee = Pk_pe - Kk*H*Pk_pe;
                obj.Pk_ee = Pk_pe - Kk*Sk*Kk';  % 这一步应该可以不要了，因为后面会求群的协方差矩阵，里面包含了这个

                obj.fplife = obj.track.mfpTracer.fplifeMax;
                obj.isValidAss = true;
                obj.mNum = obj.mNum + 1;
                % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye]
                obj.points = [obj.points; [point(1, :), obj.Xk_e']];
            end
        end


        function [Kk, Sk, Zk_p, Pz, Pxz, isExist] = update(obj, point, dr, dtheta, dvr, nowTime)
            % 状态更新,此方法会完成状态跟新，如果point为空则为记忆，若生命值为0则从track中删除该特征点
            % point: 用于状态更新的量测[1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster; ...]
            % dr: 距离量测方差
            % dtheta: 角度量测方差
            % dvr: 速度量测方差
            % nowTime: 当前时刻

            % 量测误差协方差矩阵
            R = [dr, 0, 0;
                 0, dvr, 0;
                 0, 0, dtheta];
            Sk = zeros([3, 3]);

            isExist = true;  % 标记当前特征点是否还存在
            if isempty(point)
                obj.fplife = obj.fplife - 1;
                obj.isValidAss = false;
                if obj.fplife == 0
                    isExist = false;
                    obj.track.deleteFp(obj.id);
                end
                r = norm([obj.Xk_e(1,1), obj.Xk_e(3,1)]);
                theta = atan2(obj.Xk_e(3,1), obj.Xk_e(1,1));
                vr = obj.Xk_e([2,4],1)'*obj.Xk_e([1,3],1)/r;
                x = obj.Xk_e(1,1);
                y = obj.Xk_e(3,1);
                vx = obj.Xk_e(2,1);
                vy = obj.Xk_e(4,1);
                % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye]
                obj.points = [obj.points; [nowTime, r, theta, vr, x, y, vx, vy, 0, x, y, -1, obj.Xk_e']];
                Kk = zeros([4,3]);
            else
                Zk = point(1, 2:4)';
                Xk_p = obj.Xk_e;  % 下一个状态预测值
                Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵

                % 这一步放到track里面去做，做群体的
                [Zk_p, Pz, Pxz] = obj.CuT(@obj.h, Xk_p, Pk_pe, 3);  % 量测预测、方差、以及和原状态的协方差,通过容积变换计算
    
                Vk = Zk - Zk_p;  % 新息（量测残差）
%                 Sk = H*Pk_pe*H'+R;  % 新息协方差
                Sk = Pz + R;  % 新息协方差,Pz就是量测预测的协方差，通过不敏变换计算可得
    
%                 Kk = Pk_pe*H'/Sk;  % 卡尔曼增益
                Kk = Pxz/Sk;  % Pxz是Xk_pe和Zk_p之间的协方差矩阵,通过不敏变换计算得到
                obj.Xk_e = Xk_p + Kk*Vk;
%                 obj.Pk_ee = Pk_pe - Kk*H*Pk_pe;
                obj.Pk_ee = Pk_pe - Kk*Sk*Kk';

                obj.fplife = obj.track.mfpTracer.fplifeMax;
                obj.isValidAss = true;
                obj.mNum = obj.mNum + 1;
                % [1t, 2r, 3theta, 4vr, 5x, 6y, 7vx, 8vy, 9id, 10X, 11Y, 12cluster, 13Xe, 14Vxe, 15Ye, 16Vye]
                obj.points = [obj.points; [point(1, :), obj.Xk_e']];
            end
        end

        function Zk = h(~, Xk_e)
          % 非线性变换函数
          % Xk_e: 原状态[x, vx, y, vy]'
          x = Xk_e(1,1);
          vx = Xk_e(2,1);
          y = Xk_e(3,1);
          vy = Xk_e(4,1);
        
          r = sqrt(x*x+y*y);
          theta = atan2(y, x);
          vr = [x, y]*[vx, vy]'/norm([x, y]);
        
          Zk = [r, theta, vr]';
        end
        
        
        function [Zk_p, Pz, Pxz] = UT(~, h, Xk_p, Pk_pe, Zdim)
          % 不敏变换
          % h: 非线性变换关系
          % Xk_p: 原状态期望值[x, vx, y, vy]'
          % Pk_pe: 原状态误差协方差矩阵
          % Zdim: 量测维数

          alpha = 0.001;  % 通常情况取一小正值
          beta = 2;  % 高斯情况下beta最优值为2
          tau = 0; % tau通常取0
        
          N = size(Xk_p, 1);
          X=[];  % 样本序列
          Wm = [];  % 一阶权重
          Wc = [];  % 二阶权重
          Z = [];  % 每一个样本对应线性变换后的值
          Zk_p = zeros([Zdim, 1]);
          Pz = zeros([Zdim, Zdim]);
          Pxz = zeros([N, Zdim]);
        
          lambda = alpha^2*(N+tau) - N;
          P=(chol((N+lambda)*Pk_pe))';  % 取列的话这个地方就要转置
          for n=0:1:(2*N)
              if n == 0
                  X = [X, Xk_p];
                  Wm = [Wm, lambda/(lambda+N)];
                  Wc = [Wc, lambda/(lambda+N)+1-alpha^2+beta];
              else
                  if n<=N
                      X = [X, Xk_p+P(:, n)];
                  else
                      X = [X, Xk_p-P(:, n-N)];
                  end
        
                  Wm = [Wm, 0.5/(lambda+N)];
                  Wc = [Wc, 0.5/(lambda+N)];
              end
              Z = [Z, h(X(:,n+1))];
        
              Zk_p = Zk_p + Wm(n+1)*Z(:, n+1);
          end
        
          for n=0:1:N*2
              Pz = Pz + Wc(n+1)*(Z(:, n+1) - Zk_p)*(Z(:, n+1) - Zk_p)';
              Pxz = Pxz + Wc(n+1)*(X(:, n+1) - Xk_p)*(Z(:, n+1) - Zk_p)';
          end
        end


        function [Zk_p, Pz, Pxz] = CuT(~, h, Xk_p, Pk_pe, Zdim)
          % 容积变换
          % h: 非线性变换关系
          % Xk_p: 原状态期望值[x, vx, y, vy]'
          % Pk_pe: 原状态误差协方差矩阵
          % Zdim: 量测维数
        
          N = size(Xk_p, 1);
        
          X = Xk_p + chol(Pk_pe)'*[eye(N), -eye(N)];  % 容积点（样本点s）
          W = 0.5/N;  % 权重
          
          Z = [];
          Zk_p = zeros([Zdim, 1]);
          Pz = zeros([Zdim, Zdim]);
          Pxz = zeros([N, Zdim]);
          for n = 1:1:2*N
              Z = [Z, h(X(:,n))];
              Zk_p = Zk_p + W*Z(:, n);
        
              Pz = Pz + W*Z(:, n)*Z(:, n)';
              Pxz = Pxz + W*X(:,n)*Z(:, n)';
          end
        
          Pz = Pz - Zk_p*Zk_p';
          Pxz = Pxz - Xk_p*Zk_p';
        end

    end

    methods (Static)
        function id = geId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
            ID = varargin{1};
            return;
          end
          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end


end

